
// import oimo.collision.raycast.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConvexGeometry } from "./ConvexGeometry";
import { GeometryType } from "./GeometryType";
import { MathUtil } from "../../common/MathUtil";
import { M } from "../../common/M";
import { Aabb } from "./Aabb";
import { Transform } from "../../common/Transform";
import { Vec3 } from "../../common/Vec3";
import { RayCastHit } from "./RayCastHit";

/**
 * A capsule collision geometry aligned with the y-axis.
 */

export class CapsuleGeometry extends ConvexGeometry {
	_radius: number;
	_halfHeight: number;

	/**
	 * Creates a capsule collision geometry of radius `radius` and half-height `halfHeight`.
	 */
	constructor(radius: number, halfHeight: number) {
		super(GeometryType._CAPSULE);
		this._radius = radius;
		this._halfHeight = halfHeight;
		this._gjkMargin = this._radius;
		this._updateMass();
	}

	/**
	 * Returns the radius of the capsule.
	 */
	public getRadius(): number {
		return this._radius;
	}

	/**
	 * Returns the half-height of the capsule.
	 */
	public getHalfHeight(): number {
		return this._halfHeight;
	}

	public _updateMass(): void {
		var r2: number = this._radius * this._radius;
		var hh2: number = this._halfHeight * this._halfHeight;

		var cylinderVolume: number = MathUtil.TWO_PI * r2 * this._halfHeight;
		var sphereVolume: number = MathUtil.PI * r2 * this._radius * 4 / 3;
		this._volume = cylinderVolume + sphereVolume;

		var invVolume: number = this._volume == 0 ? 0 : 1 / this._volume;

		var inertiaY: number = invVolume * (
			cylinderVolume * r2 * 0.5 +
			sphereVolume * r2 * 0.4
		);

		var inertiaXZ: number = invVolume * (
			cylinderVolume * (r2 * 0.25 + hh2 / 3) +
			sphereVolume * (r2 * 0.4 + this._halfHeight * this._radius * 0.75 + hh2)
		);

		M.mat3_diagonal(this._inertiaCoeff, inertiaXZ, inertiaY, inertiaXZ);
	}

	public _computeAabb(aabb: Aabb, tf: Transform): void {
		var radVec: Vec3 = new Vec3(this._radius, this._radius, this._radius);

		var axis: Vec3 = new Vec3();
		M.mat3_getCol(axis, tf._rotation, 1);
		axis.absEq();
		M.vec3_scale(axis, axis, this._halfHeight);

		M.vec3_add(radVec, radVec, axis);
		M.vec3_sub(aabb._min, tf._position, radVec);
		M.vec3_add(aabb._max, tf._position, radVec);
	}

	public computeLocalSupportingVertex(dir: Vec3, out: Vec3): void {
		if (dir.y > 0) {
			out.init(0, this._halfHeight, 0);
		} else {
			out.init(0, -this._halfHeight, 0);
		}
	}

	public _rayCastLocal(begin: Vec3, end: Vec3, hit: RayCastHit): boolean {
		var p1x: number = M.vec3_get(begin, 0);
		var p1y: number = M.vec3_get(begin, 1);
		var p1z: number = M.vec3_get(begin, 2);
		var p2x: number = M.vec3_get(end, 0);
		var p2y: number = M.vec3_get(end, 1);
		var p2z: number = M.vec3_get(end, 2);
		var halfH: number = this._halfHeight;
		var dx: number = p2x - p1x;
		var dy: number = p2y - p1y;
		var dz: number = p2z - p1z;

		// XZ
		var tminxz: number = 0;
		var tmaxxz: number = 1;
		var a: number = dx * dx + dz * dz;
		var b: number = p1x * dx + p1z * dz;
		var c: number = (p1x * p1x + p1z * p1z) - this._radius * this._radius;
		var D: number = b * b - a * c;
		if (D < 0) return false;
		var t: number;
		if (a > 0) {
			var sqrtD: number = MathUtil.sqrt(D);
			tminxz = (-b - sqrtD) / a;
			tmaxxz = (-b + sqrtD) / a;
			if (tminxz >= 1 || tmaxxz <= 0) return false;
		} else {
			if (c >= 0) return false;
			tminxz = 0;
			tmaxxz = 1;
		}

		var crossY: number = p1y + dy * tminxz;
		var min: number;

		if (crossY > -halfH && crossY < halfH) {
			if (tminxz > 0) {
				// hit: side
				min = tminxz;
				hit.normal.init(p1x + dx * min, 0, p1z + dz * min).normalize();
				hit.position.init(p1x + min * dx, crossY, p1z + min * dz);
				hit.fraction = min;
				return true;
			}
			return false;
		}

		var sphereY: number = crossY < 0 ? -halfH : halfH;
		var spherePos: Vec3 = new Vec3(0, sphereY, 0);
		var sphereToBegin: Vec3 = new Vec3();
		M.vec3_sub(sphereToBegin, begin, spherePos);

		// sphere test
		var d: Vec3 = new Vec3();
		M.vec3_sub(d, end, begin);

		a = M.vec3_dot(d, d);
		b = M.vec3_dot(sphereToBegin, d);
		c = M.vec3_dot(sphereToBegin, sphereToBegin) - this._radius * this._radius;

		D = b * b - a * c;
		if (D < 0) return false;

		var t: number = (-b - MathUtil.sqrt(D)) / a;
		if (t < 0 || t > 1) return false;

		var hitPos: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(hitPos, sphereToBegin, d, t);
		var hitNormal: Vec3 = hitPos.normalized();

		// sphere-oriented pos -> local pos
		M.vec3_add(hitPos, hitPos, spherePos);

		hit.position.copyFrom(hitPos);
		hit.normal.copyFrom(hitNormal);
		hit.fraction = t;

		return true;
	}

}
